// Copyright (c) 2025 Shenzhen Kaihong Digital Industry Development Co., Ltd.
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

use std::marker::PhantomData;

use libc::{c_char, c_int, c_void, size_t};
use rerun::{ external::clap::{self, ValueEnum}, Points3D, Position3D, Vec3D};

pub struct PointXYZ;
pub struct PointXYZI;

#[derive(Debug, Clone, ValueEnum)]
pub enum DataFormat {
    XYZ,
    XYZI
}

#[repr(C)]
#[derive(Clone, Copy)]
pub union Points3DOrIntensity {
    pub position: Position3D,
    pub intensity: f32,
}

pub trait PclPoint {
    unsafe fn new_cloud() -> *mut c_void;
    unsafe fn delete_cloud(cloud: *mut c_void);
    unsafe fn load_pcd_file(
        file_name: *const c_char,
        name_len: size_t,
        cloud: *mut c_void,
        size: *mut size_t
    ) -> c_int;
}

pub struct PointCloudPtr<Point: PclPoint> {
    ptr: *mut c_void,
    size: usize,
    _mark: PhantomData<Point>,
}

impl<Point: PclPoint> Default for PointCloudPtr<Point> {
    fn default() -> Self {
        Self::new()
    }
}

impl<Point: PclPoint> PointCloudPtr<Point> {
    pub fn new() -> Self {
        let ptr = unsafe { Point::new_cloud() };
        Self { ptr, size: 0, _mark: PhantomData }
    }

    pub fn from_pcd_file(file_name: &str) -> Option<Self> {
        let mut cloud = Self::new();
        let ret = unsafe {
            Point::load_pcd_file(
                file_name.as_ptr() as *mut c_char,
                file_name.len(),
                cloud.ptr,
                &mut cloud.size as *mut usize
            )
        };
        if ret == 0 {
            Some(cloud)
        } else {
            None
        }
    }
}

impl<Point: PclPoint> Drop for PointCloudPtr<Point> {
    fn drop(&mut self) {
        unsafe { Point::delete_cloud(self.ptr) };
    }
}

#[link(name = "rerun_tools_cpp")]
extern {
    fn new_xyz_point_cloud() -> *mut c_void;
    fn new_xyzi_point_cloud() -> *mut c_void;
    fn delete_xyz_point_cloud(cloud: *mut c_void);
    fn delete_xyzi_point_cloud(cloud: *mut c_void);

    fn load_xyz_pcd_file(
        file_name: *const c_char,
        name_len: size_t,
        cloud: *mut c_void,
        size: *mut size_t
    ) -> c_int;
    fn load_xyzi_pcd_file(
        file_name: *const c_char,
        name_len: size_t,
        cloud: *mut c_void,
        size: *mut size_t
    ) -> c_int;

    fn cloud_xyz_to_vecs3d(cloud: *mut c_void, vecs: *mut c_void);
    fn cloud_xyzi_to_vecs3d(cloud: *mut c_void, vecs: *mut c_void);
    fn cloud_xyzi_to_position3d(cloud: *mut c_void, vecs: *mut c_void);
    fn cloud_xyzi_to_intensity(cloud: *mut c_void, vecs: *mut c_void);
}

impl PclPoint for PointXYZ {
    unsafe fn new_cloud() -> *mut c_void {
        new_xyz_point_cloud()
    }

    unsafe fn delete_cloud(cloud: *mut c_void) {
        delete_xyz_point_cloud(cloud)
    }

    unsafe fn load_pcd_file(
        file_name: *const c_char,
        name_len: size_t,
        cloud: *mut c_void,
        size: *mut size_t,
    ) -> c_int {
        load_xyz_pcd_file(file_name, name_len, cloud, size)
    }
}

impl PclPoint for PointXYZI {
    unsafe fn new_cloud() -> *mut c_void {
        new_xyzi_point_cloud()
    }

    unsafe fn delete_cloud(cloud: *mut c_void) {
        delete_xyzi_point_cloud(cloud)
    }

    unsafe fn load_pcd_file(
        file_name: *const c_char,
        name_len: size_t,
        cloud: *mut c_void,
        size: *mut size_t,
    ) -> c_int {
        load_xyzi_pcd_file(file_name, name_len, cloud, size)
    }
}

impl PointCloudPtr<PointXYZ> {
    pub fn to_vecs3d(&self) -> Vec<Vec3D> {
        let mut vecs = vec![Vec3D::ZERO; self.size];
        unsafe { 
            PointXYZ::cloud_to_vecs3d(self.ptr, vecs.as_mut_ptr() as *mut c_void)
        };
        vecs
    }

    pub fn to_positions3d(&self) -> Vec<Position3D> {
        let vecs = self.to_vecs3d();
        unsafe { std::mem::transmute(vecs) }
    }

    pub fn to_vecs3d_bytes(&self) -> Vec<u8> {
        let mut vecs = vec![Vec3D::ZERO; self.size * size_of::<Vec3D>()];
        unsafe { PointXYZ::cloud_to_vecs3d(self.ptr, vecs.as_mut_ptr() as *mut c_void) };
        unsafe { std::mem::transmute(vecs) }
    }

    pub fn to_points3d(&self) -> Points3D {
        Points3D {
            positions: self.to_positions3d(),
            radii: None,
            colors: None,
            labels: None,
            class_ids: None,
            keypoint_ids: None,
        }
    }
}

impl PointCloudPtr<PointXYZI> {
    pub fn to_vecs3d_bytes_with_intensity(&self) -> Vec<u8> {
        let size_of_element = std::mem::size_of::<Position3D>() + std::mem::size_of::<f32>();
        let mut vecs = vec![0f32; self.size * size_of_element];
        unsafe { 
            PointXYZI::cloud_to_vecs3d_with_intensity(
                self.ptr,
                vecs.as_mut_ptr() as *mut c_void,
            )
        };
        unsafe { std::mem::transmute(vecs) }
    }

    pub fn position_to_vec3d(&self) -> Vec<Position3D> {
        let mut vecs = vec![Vec3D::ZERO; self.size];
        unsafe {
            PointXYZI::cloud_xyzi_to_position3d(
                self.ptr, 
                vecs.as_mut_ptr() as *mut c_void
            )
        };
        unsafe { std::mem::transmute(vecs) }
    }

    pub fn to_intensity(&self) -> Vec<f32> {
        let mut vecs = vec![0f32; self.size];
        unsafe {
            PointXYZI::cloud_xyzi_to_intensity(
                self.ptr,
                vecs.as_mut_ptr() as *mut c_void
            )
        };
        vecs
    }
}

impl PointXYZ {
    unsafe fn cloud_to_vecs3d(cloud: *mut c_void, vecs: *mut c_void) {
        cloud_xyz_to_vecs3d(cloud, vecs)
    }
}

impl PointXYZI {
    unsafe fn cloud_to_vecs3d_with_intensity(cloud: *mut c_void, vecs: *mut c_void) {
        cloud_xyzi_to_vecs3d(cloud, vecs)
    }

    unsafe fn cloud_xyzi_to_position3d(cloud: *mut c_void, vecs: *mut c_void) {
        cloud_xyzi_to_position3d(cloud, vecs);
    }

    unsafe fn cloud_xyzi_to_intensity(cloud: *mut c_void, vecs: *mut c_void) {
        cloud_xyzi_to_intensity(cloud, vecs);
    }
}